#include <edds/rtps/interfaces/IContentFilterFactory.hpp>
#include <rtps/content_filter/DDSSQLFilter/DDSFilterFactory.hpp>
#include <edds/rtps/interfaces/ContentFilterUtils.hpp>
#include <edds/rtps/common/CDRMessage_t.h>
#include <ertps/qos/ParameterTypes.h>
#include <edds/dds/core/policy/ParameterList.hpp>

#include <deps/common/md5.h>

namespace evbs {
namespace ertps {
namespace rtps {

void ContentFilterUtils::compute_signature(const vbsutil::xmlparser::ContentFilterProperty& filter_property,
                                           std::array<uint8_t, 16>& filter_signature) {
    vbsutil::MD5 md5_rtps;

    md5_rtps.init();

    // Add content_filtered_topic_name
    {
        const char* str = filter_property.content_filtered_topic_name.c_str();
        vbsutil::MD5::size_type slen = static_cast<vbsutil::MD5::size_type>(strlen(str) + 1);
        md5_rtps.update(str, slen);
    }
    // Add related_topic_name
    {
        const char* str = filter_property.related_topic_name.c_str();
        vbsutil::MD5::size_type slen = static_cast<vbsutil::MD5::size_type>(strlen(str) + 1);
        md5_rtps.update(str, slen);
    }
    // Add filter_class_name
    {
        const char* str = filter_property.filter_class_name.c_str();
        vbsutil::MD5::size_type slen = static_cast<vbsutil::MD5::size_type>(strlen(str) + 1);
        md5_rtps.update(str, slen);
    }
    // Add filter_expression
    {
        const char* str = filter_property.filter_expression.c_str();
        vbsutil::MD5::size_type slen = static_cast<vbsutil::MD5::size_type>(strlen(str) + 1);
        md5_rtps.update(str, slen);
    }
    // Add expression_parameters
    for (const auto& param : filter_property.expression_parameters) {
        const char* str = param.c_str();
        vbsutil::MD5::size_type slen = static_cast<vbsutil::MD5::size_type>(strlen(str) + 1);
        md5_rtps.update(str, slen);
    }
    md5_rtps.finalize();

    std::copy_n(md5_rtps.digest, filter_signature.size(), filter_signature.begin());
}

bool ContentFilterUtils::check_filter_signature(const ertps::rtps::CacheChange_t& change,
                                                const std::array<uint8_t, 16>& filter_signature, bool& filter_result) {
    // Find PID_CONTENT_FILTER_INFO on inline QoS
    if (0 == change.inline_qos.length || nullptr == change.inline_qos.data) {
        return false;
    }

    bool found = false;

    // This will be called for every parameter found on the inline QoS of the received change.
    // If a ContentFilterInfo parameter is found, it will traverse the filter signatures trying to find one that
    // matches our own signature, in which case found is set to true, and filter_result is set to the result present
    // on the ContentFilterInfo parameter for that signature.
    auto parameter_process = [&](ertps::rtps::CDRMessage_t* msg, const ParameterId_t pid, uint16_t plength) {
        if (vbsutil::xmlparser::PID_CONTENT_FILTER_INFO == pid) {
            uint32_t num_bitmaps = 0;
            uint32_t num_signatures = 0;
            bool valid;

            // Validate and consume length for numBitmaps and numSignatures
            valid = 8 < plength;
            if (!valid) {
                return true;
            }
            plength -= 8;

            // Read and validate numBitmaps
            valid &= ertps::rtps::CDRMessage::readUInt32(msg, &num_bitmaps);
            valid &= num_bitmaps * 4 <= plength;
            if (!valid || 0 == num_bitmaps) {
                return true;
            }

            // Save starting position of bitmaps and skip them
            uint32_t bitmap_pos = msg->pos;
            msg->pos += num_bitmaps * 4;
            plength -= static_cast<uint16_t>(num_bitmaps * 4);

            // Read and validate numSignatures
            valid &= ertps::rtps::CDRMessage::readUInt32(msg, &num_signatures);
            valid &= num_signatures * 16 <= plength;
            if (!valid || 0 == num_signatures || ((num_signatures + 31) / 32) != num_bitmaps) {
                return true;
            }

            // Lookup our own signature
            uint32_t i;
            for (i = 0; i < num_signatures; ++i) {
                if (std::equal(filter_signature.begin(), filter_signature.end(), msg->buffer + msg->pos)) {
                    found = true;
                    break;
                }
                msg->pos += 16;
            }

            // Signature found, set filter result from bitmap
            if (found) {
                uint32_t bitmap_idx = i / 32;
                uint32_t bitmask = 1 << (31 - (i & 31));
                uint32_t bitmap = 0;

                msg->pos = bitmap_pos + bitmap_idx * 4;
                ertps::rtps::CDRMessage::readUInt32(msg, &bitmap);
                filter_result = 0 != (bitmap & bitmask);
            }
        }

        return true;
    };

    uint32_t qos_size = 0;
    ertps::rtps::CDRMessage_t msg(change.inline_qos);
    edds::dds::ParameterList::readParameterListfromCDRMsg(msg, parameter_process, false, qos_size);
    return found;
}

IContentFilterFactory* IContentFilterFactory::get_default_filter_factory() {
    static DDSSQLFilter::DDSFilterFactory dds_sql_filter_factory_;
    return &dds_sql_filter_factory_;
}

}  // namespace rtps
}  // namespace ertps
}  // namespace evbs